﻿using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.IO;
using System.Linq;

namespace ImryStefanVRP
{

    class Program
    {
        public static Random rand;

        // Slower if true
        public static bool DEBUG = false;

        // Global variable, for convenience
        public static CVRPData vrpData;

        static void Main(string[] args)
        {
            Console.WriteLine("Welcome to our Neighborhood Search Meta-Heuristic implementation");

            //add input parameter on console for time
            double timeinAO;
            double timeinRandsavings;
            double increment;
            double inputtime;

            // If there are command line arguments, the first one is the file name
            String inputFileName;
            if (args.Length != 0)
                inputFileName = args[0];
            else
                inputFileName = "Golden_06.vrp";

            // Create the parser object, and read in all the data from file. This object can read ANY TSPLIB file. Code borrowed from HeuristicLab.
            TSPLIBParser myParser = new TSPLIBParser(inputFileName);
            myParser.Parse();

            // Create the global vrp data object, using the parsed data
            Program.vrpData = new CVRPData(myParser);
                                               
            //Console.WriteLine("Default parameter setting? '1 for yes, 0 for no'");
            //string inputsettting = Console.ReadLine();
            //double setting=Convert.ToDouble(inputsettting);
            double setting = 1;
            if(setting==0)
            {
                Console.WriteLine("Please input the total runtime in minutes (the minimum runtime is 4 minutes):");
                string time=Console.ReadLine();
                inputtime = Convert.ToDouble(time);

                if (inputtime >= 8)
                {
                    if (vrpData.Dimension >= 200)
                    {
                        timeinAO = 0.5 * inputtime;
                        timeinRandsavings = 1.5;
                        increment = 0.4;
                    }
                    else
                    {
                        timeinAO = 0.3*inputtime;
                        timeinRandsavings = 1;
                        increment = 0.3;
                    }

                }
                else
                {
                    if (vrpData.Dimension >= 200)
                    {
                        timeinAO = 0.3 * inputtime;
                        timeinRandsavings = 0.7;
                        increment = 0.4;
                    }
                    else
                    {
                        timeinAO = 0.25*inputtime;
                        timeinRandsavings = 0.35;
                        increment = 0.3;
                    }
                }
            }
            else
            {
                inputtime = 5;

                if (vrpData.Dimension >= 200)
                {
                    timeinAO = 1;
                    timeinRandsavings = 1;
                    increment = 0.4;
                }
                else
                {
                    timeinAO = 2;
                    timeinRandsavings = 0.1;
                    increment = 0.3;
                }
            }

            Console.WriteLine("Meta-Heuristic is running.");

            rand = new Random();
            Stopwatch stopWatch = new Stopwatch();

            stopWatch.Start();

            
                int AOCount = 0;
                int SavingsImproveCount = 0;
                int SJCapCount = 0;
                int SJCount = 0;
                int orgCount = 0;

            double maxminutes = inputtime;

            //savings_initialSolution: the initial solution
            CVRPSolution savings_initialSolution = SavingsMethodHeuristic(Program.vrpData);



            // for VISUALIZATION - DO NOT DELETE
            Console.WriteLine("Do you want visualization? 1 for Yes, 0 for No");
            string inputvisual = Console.ReadLine();
            double visual = Convert.ToDouble(inputvisual);
            if (visual == 1)
            {
                CVRPData visual_data = new CVRPData(Program.vrpData);
                Display f = new Display(savings_initialSolution, visual_data);
                f.ShowDialog();
            }


            CVRPSolution bestcurrentSolution = new CVRPSolution(savings_initialSolution);
            //CVRPSolution bestcurrentSolution = SavingsMethodHeuristic(Program.vrpData);

            //test to see if vehicle drop can happen, if not - it returns the same as above
            bestcurrentSolution = NeighboorhoodSearches.RouteReduction(bestcurrentSolution, Program.vrpData);
           

            while (stopWatch.Elapsed.TotalMinutes < timeinRandsavings)
            {
                CVRPSolution randSolution = RandSavingsMethodHeuristic(Program.vrpData);
                if (randSolution.totalCost < bestcurrentSolution.totalCost)
                {
                    bestcurrentSolution = new CVRPSolution(randSolution);
                    //Console.WriteLine("Randomized better cost found.\nTotalcost :  " + bestcurrentSolution.totalCost);
                    bestcurrentSolution.FeasibilityCheck();
                }
            }



            //initialize parameters 
            double lambdaF = 0; //we will store the final parameter values here
            double muF = 0;
            double nuF = 0;
            double totalCostInitial = 0;
            int numRoutesInitial=0;

            //initialize solutions 
            CVRPSolution AOSolution = new CVRPSolution(bestcurrentSolution);  
            CVRPSolution FinalAOSolution = new CVRPSolution(bestcurrentSolution);
            CVRPSolution test0 = new CVRPSolution();
            CVRPSolution test1 = new CVRPSolution();
            CVRPSolution test2 = new CVRPSolution();
            CVRPSolution mincurrentSolution = new CVRPSolution();
            CVRPSolution tempSolution = new CVRPSolution(bestcurrentSolution);


            test0 = new CVRPSolution(NeighboorhoodSearches.saving_improve(vrpData, bestcurrentSolution));

            //"2" can change. Routes are ranked based on demand. Currently picked the lowest two and resolve with these two routes. 
            //picks a number of routes to re-sort using savings again just with those routes (routes sorted based on capacity)
            test1 = new CVRPSolution(NeighboorhoodSearches.superjump_capacity(vrpData, bestcurrentSolution, 2));

            //sort routes based on their cost/length.
            test2 = new CVRPSolution(NeighboorhoodSearches.superjump(vrpData, bestcurrentSolution, 2));
           
            //Get some new solutions from the Altinel and Oncan Version of Savings
                for (double lambdaTry = .1; lambdaTry < 2; lambdaTry += increment)  //increment lambda
                {
                    for (double muTry = 0; muTry < 2; muTry += increment)   //increment mu
                    {
                        for (double nuTry = 0; nuTry < 2; nuTry += increment)   //increment nu
                        {
                            if (stopWatch.Elapsed.TotalMinutes >= timeinAO)
                            { break; }
                            AOSolution = new CVRPSolution(SavingsMethodHeuristicAltinel(Program.vrpData, lambdaTry, muTry, nuTry));
                            if (AOSolution.totalCost < FinalAOSolution.totalCost)  //then we know the new solution is better than the old one
                            {
                                FinalAOSolution = new CVRPSolution(AOSolution);   //then we set it as the new best solution                              
                                lambdaF = lambdaTry;
                                muF = muTry;
                                nuF = nuTry;
                                if (stopWatch.Elapsed.TotalMinutes >= timeinAO)
                                { break; }

                            }
                            if (stopWatch.Elapsed.TotalMinutes >= timeinAO)
                            { break; }
                        }
                        if (stopWatch.Elapsed.TotalMinutes >= timeinAO)
                        { break; }
                    }
                    if (stopWatch.Elapsed.TotalMinutes >= timeinAO)
                    { break; }
                }

                // Savings rebuild
                test0 = new CVRPSolution(NeighboorhoodSearches.saving_improve(vrpData, bestcurrentSolution));

                //"2" can change. Routes are ranked based on demand. Currently picked the lowest two and resolve with these two routes. 
                //Sometimes highest might give us the better routes. 
                //picks a number of routes to resort using savings again just with those routes (routes sorted based on capacity)
                test1 = new CVRPSolution(NeighboorhoodSearches.superjump_capacity(vrpData, bestcurrentSolution, 2));

                //sort routes based on their cost/length.
                test2 = new CVRPSolution(NeighboorhoodSearches.superjump(vrpData, bestcurrentSolution, 2));

                //find best solution of test0, 1 and 2 and store it as tempsolution
                if (test0.totalCost < test1.totalCost) //   test0<test1
                {
                    if (test0.totalCost < test2.totalCost)  //  test0<test1 and test2
                    {
                        tempSolution = new CVRPSolution(test0);   //then test0 is min   (savings_improve)
                        SavingsImproveCount ++;
                    }
                    else
                    {
                        tempSolution = new CVRPSolution(test2);    // test2< test0<test1     (super jump)
                        
                    }
                }
                else    //this means test1 < test0
                {
                    if (test1.totalCost < test2.totalCost)  //check to see if test1<test2    
                    {
                        tempSolution = new CVRPSolution(test1);   //then test1 is min     (superjump_capacity)
                        SJCapCount ++;
                    }
                    else   //then test2<test1<test0
                    {
                        tempSolution = new CVRPSolution(test2);           //   (superjump)
                        SJCount ++ ;
                    }
                }

                //compare best test solution (now called tempsolution) to the old solution, and keep it if better
                if (tempSolution.totalCost < bestcurrentSolution.totalCost)   //check if less than old solutions' value
                {
                    mincurrentSolution = new CVRPSolution(tempSolution);   
                }
                else
                {
                    mincurrentSolution = new CVRPSolution(bestcurrentSolution);    //orginal savings solution is best
                    orgCount ++;
                }


                //compare mincurrentSolution to the best Savings from the new Savings Method value
                if (FinalAOSolution.totalCost < mincurrentSolution.totalCost)   //check if less than old solutions' value
                { 
                    mincurrentSolution = new CVRPSolution(FinalAOSolution);    //AO solution is best
                    AOCount ++;
                }

                totalCostInitial = mincurrentSolution.totalCost;
                numRoutesInitial = mincurrentSolution.listOfRoutes.Count();

            //Using simulated anealing algorithm
            CVRPSolution mySolution = SimulatedAnnealing(mincurrentSolution, maxminutes, stopWatch, Program.vrpData);
            double totalCostFinal = mySolution.totalCost;
            int numRoutesFinal = mySolution.listOfRoutes.Count();
           
            // Check if solution makes sense
            mySolution.FeasibilityCheck();



            // Calculate the total time
            stopWatch.Stop();
            TimeSpan ts = stopWatch.Elapsed;

            // Output the solution to file
            mySolution.WriteToFile(inputFileName + ".opt", ts,lambdaF,muF,nuF);
            

            // Output the statistics to csv
            string teamName = "ImryStefan_BES";
            string outputFile = teamName + ".csv";
            using (StreamWriter writer = new StreamWriter(outputFile, true))
            {
                writer.WriteLine(teamName + ", " + inputFileName + ", " + totalCostInitial + ", " + numRoutesInitial + ", " + totalCostFinal + ", " + numRoutesFinal);
                writer.Close();
            }


            // Uncomment to run from Visual Studio
            //Console.WriteLine("Press any key to exit.");
            //Console.ReadKey();
            //Console.WriteLine("AOCount:" +AOCount);
            //Console.WriteLine("SavingsImproveCount:" + SavingsImproveCount);
            //Console.WriteLine("SJCapCount:" + SJCapCount);
            //Console.WriteLine("SJCount:" + SJCount);
            //Console.WriteLine("orgCount:" + orgCount);



            // for VISUALIZATION - DO NOT DELETE
            //Console.WriteLine("Do you want visualization? 1 for Yes, 0 for No");
            //string inputvisual = Console.ReadLine();
            //double visual = Convert.ToDouble(inputvisual);
            //if (visual == 1)
            //{
            //    CVRPData visual_data = new CVRPData(Program.vrpData);
            //    Display f = new Display(mySolution, visual_data);
            //    f.ShowDialog();
            //}
        }

        private static CVRPSolution SimulatedAnnealing(CVRPSolution currentSolution, double maxMinutes, Stopwatch stopWatch, CVRPData data)
        {
            CVRPSolution currentBestSolution = currentSolution.Clone();
            TimeSpan timeOfLastImprovement = stopWatch.Elapsed;
            double temperature = 1;
            int rebuildcount = 0;
            while (true)
            {
                if (stopWatch.Elapsed.TotalMinutes > maxMinutes)
                { break; }
                ChangeInSolution theChangeInSolution = NeighboorhoodSearches.GetGoodSolutionInANeighborhood(currentSolution);

                if (currentSolution.totalCost < currentBestSolution.totalCost - 1e-6) // Trial cost is better that current best cost. Accept and decrease temperature.
                {
                    //Console.WriteLine("Accepted as new best = " + currentSolution.totalCost);
                    currentBestSolution = currentSolution.Clone();
                    //The did not improve the temperature according to the SA rule. 
                    temperature = temperature * 0.9;
                    timeOfLastImprovement = stopWatch.Elapsed;
                }
                //undo is not necessary, you could just calculate the cost first.
                else if (currentSolution.totalCost > theChangeInSolution.totalCostBeforeChange && rand.NextDouble() > temperature) // Trial cost is bad, and we are out of luck. Don't accept the move (undo it)
                    currentSolution.UndoChange(theChangeInSolution);

                if (stopWatch.Elapsed.TotalMinutes > maxMinutes)
                { break; }

                // Hopfully helpful in getting out of local minima, if time 
                if (stopWatch.Elapsed.TotalSeconds - timeOfLastImprovement.TotalSeconds > 30)
                {
                    rebuildcount = rebuildcount + 1;
                    temperature = 1;
                    timeOfLastImprovement = stopWatch.Elapsed;
                    //Console.WriteLine("Making a Super Jump!-Rebuilding 2 routes");
                    NeighboorhoodSearches.superjump(data, currentSolution, 2);

                    //shuffling in order to get out a local min, to make things worse, in hopes they will get better
                    if (rebuildcount == 3)
                    {
                        temperature = 1;
                        timeOfLastImprovement = stopWatch.Elapsed;
                       // Console.WriteLine("shuffling");
                        int n = Program.rand.Next(currentSolution.listOfRoutes.Count);
                        NeighboorhoodSearches.Shuffle(n, currentSolution);
                        rebuildcount = 0;
                    }

                }            
            }
            return currentBestSolution;
        }


        public static CVRPSolution SavingsMethodHeuristic(CVRPData data)
        {
            //Console.WriteLine("Starting the SavingsMethodHeuristic");

            //The savings list
            List<SavingsCouple> Savings = new List<SavingsCouple>();
            //Go over all possible couples once and create a coupling, we rely on the fact that the distances are symmetric
            for (int i = 1; i < data.Dimension; i++)
            {
                for (int j = 1; j < data.Dimension; j++)
                {
                    if (j != i)
                    {
                        SavingsCouple cpl = new SavingsCouple();
                        cpl.FirstNode = i;
                        cpl.SecondNode = j;
                        cpl.Saving = data.Distances[i, 0] + data.Distances[0, j] - data.Distances[i, j];
                        Savings.Add(cpl);
                    }
                }

            }

            Savings = Savings.OrderByDescending(x => x.Saving).ToList();

            // Current tour list. Populated in the following loop, and will have tours removed and added as the heuristic progresses.
            List<VehicleRoute> Tours = new List<VehicleRoute>();
            // List of nodes that are either at the start of a tour or at the end. These can be merged. Initially all nodes are in the list.
            List<int> availNodes = new List<int>();
            // Create the inital tours
            //Console.WriteLine("Start creating the initial routes");
            for (int i = 1; i < data.Dimension; i++)
            {
                VehicleRoute tour = new VehicleRoute(i);
                tour.totalDemandOnRoute = data.Demands[i];
                tour.totalDistanceOfRoute = 2 * data.Distances[0, i];
                Tours.Add(tour);
                availNodes.Add(i);
            }
            //Console.WriteLine("Finished creating the initial routes");

            //We will iterate through the savings list
            for (int i = 0; i < Savings.Count; i++)
            {
                //flag variable that indicate if both nodes in savings couple are at the extremites of a tour
                int flag = 0;

                //check if the nodes are in the extremites
                for (int j = 0; j < availNodes.Count; j++)
                    if ((availNodes[j] == Savings[i].FirstNode) || (availNodes[j] == Savings[i].SecondNode))
                        flag++;

                //If the nodes are at the extremities, try to merge
                if (flag == 2)
                {
                    //Console.WriteLine("Trying to merge since nodes are extremeties");
                    //we need to find the two tours that contain the nodes
                    //id of the tours
                    int indexOfFirstRoute = -1;
                    int indexOfSecondRoute = -1;

                    for (int j = 0; j < Tours.Count; j++)
                    {
                        //We found the tour that finishes with the first node
                        if (Tours[j].GetNodeAt(Tours[j].GetNumberOfNodesVisited()) == Savings[i].FirstNode)
                            indexOfFirstRoute = j;
                        //We found the tour that starts with the second node
                        if (Tours[j].GetNodeAt(1) == Savings[i].SecondNode)
                            indexOfSecondRoute = j;
                    }

                    // Merge routes if:
                    // 1. pair from savings finishes one route and starts another
                    // 2. its not the same route
                    // 3. capacity not violated by merging
                    if (indexOfSecondRoute >= 0 && indexOfFirstRoute >= 0 && indexOfSecondRoute != indexOfFirstRoute && Tours[indexOfFirstRoute].totalDemandOnRoute + Tours[indexOfSecondRoute].totalDemandOnRoute <= data.Capacity)
                    {
                        int newlength = Tours[indexOfFirstRoute].GetNumberOfNodesVisited() + Tours[indexOfSecondRoute].GetNumberOfNodesVisited();

                        // Create a new sequence of nodes which corresponds to the merged route. It will include nodes from both routes
                        List<int> tour = new List<int>();
                        double distCalculated = 0;
                        tour.Add(0);
                        int j = 0;
                        for (j = 0; j < newlength; j++)
                        {
                            if (j < Tours[indexOfFirstRoute].GetNumberOfNodesVisited())
                                tour.Add(Tours[indexOfFirstRoute].GetNodeAt(j + 1));
                            else
                            {
                                //subtract the length of tour A
                                int normIndex = j - Tours[indexOfFirstRoute].GetNumberOfNodesVisited();
                                tour.Add(Tours[indexOfSecondRoute].GetNodeAt(normIndex + 1));
                            }
                            distCalculated += data.Distances[tour[j], tour[j + 1]];
                        }
                        distCalculated += data.Distances[tour[j], 0];
                        tour.Add(0);
                        VehicleRoute Tour = new VehicleRoute(tour);
                        Tour.totalDistanceOfRoute = distCalculated;
                        Tour.totalDemandOnRoute = Tours[indexOfFirstRoute].totalDemandOnRoute + Tours[indexOfSecondRoute].totalDemandOnRoute;
                        //add the combined tour and remove the old ones
                        Tours.Add(Tour);

                        // If the any of the tours merged are of length greater than 2, their corresponding extreme (starting or ending) node cannot
                        // be merged anymore. It can be removed from the savings list.
                        if (Tours[indexOfFirstRoute].GetNumberOfNodesVisited() > 1)
                            availNodes.Remove(Tours[indexOfFirstRoute].GetLastNodeVisited());
                        if (Tours[indexOfSecondRoute].GetNumberOfNodesVisited() > 1)
                            availNodes.Remove(Tours[indexOfSecondRoute].GetNodeAt(1));

                        // Remove the merged routes from the route list.
                        Tours.RemoveAt(indexOfFirstRoute);
                        if (indexOfFirstRoute > indexOfSecondRoute)
                            Tours.RemoveAt(indexOfSecondRoute);
                        else
                            Tours.RemoveAt(indexOfSecondRoute - 1);
                    }
                }
            }

            //Tours now contain the solution
            CVRPSolution sol = new CVRPSolution();
            sol.listOfRoutes = new List<VehicleRoute>();

            double heuristicTotalCost = 0;
            for (int i = 0; i < Tours.Count; i++)
            {
                sol.listOfRoutes.Add(Tours[i]);
                heuristicTotalCost += data.Distances[0, sol.listOfRoutes[i].GetNodeAt(1)] + data.Distances[sol.listOfRoutes[i].GetLastNodeVisited(), 0];
                for (int j = 0; j < sol.listOfRoutes[i].GetNumberOfNodesVisited() - 1; j++)
                    heuristicTotalCost += data.Distances[sol.listOfRoutes[i].GetNodeAt(j + 1), sol.listOfRoutes[i].GetNodeAt(j + 2)];
            }
            sol.totalCost = heuristicTotalCost;
            //Console.WriteLine("Heuristic done.\nTotalcost : " + sol.totalCost);
            return sol;
        }


        //Altine & Oncan 
        public static CVRPSolution SavingsMethodHeuristicAltinel(CVRPData data,double lambda, double mu, double nu)  //we are passing the parameters here
        {
            //Console.WriteLine("Starting the SavingsMethodHeuristic-Altinel");

            //The savings list
            List<SavingsCouple> Savings = new List<SavingsCouple>();
            //Go over all possible couples once and create a coupling, we rely on the fact that the distances are symmetric
            double totalDemand;
            totalDemand = 0;

            for (int i = 1; i < data.Dimension; i++)
            {
                totalDemand += data.Demands[i];
            }
            
            double averageDemand;
            averageDemand = totalDemand / (data.Dimension - 1);   //compute the average demand



            for (int i = 1; i < data.Dimension; i++)
            {
                for (int j = 1; j < data.Dimension; j++)
                {
                    if (j != i)
                    {
                        SavingsCouple cpl = new SavingsCouple();
                        cpl.FirstNode = i;
                        cpl.SecondNode = j;
                        cpl.Saving = data.Distances[i, 0] + data.Distances[0, j] - lambda*data.Distances[i, j]+mu*Math.Abs(data.Distances[0,i]-data.Distances[j,0])+nu*((data.Demands[i]+data.Demands[j])/averageDemand);
                        Savings.Add(cpl);
                    }
                }

            }

            Savings = Savings.OrderByDescending(x => x.Saving).ToList();

            // Current tour list. Populated in the following loop, and will have tours removed and added as the heuristic progresses.
            List<VehicleRoute> Tours = new List<VehicleRoute>();
            // List of nodes that are either at the start of a tour or at the end. These can be merged. Initially all nodes are in the list.
            List<int> availNodes = new List<int>();
            // Create the inital tours
            //Console.WriteLine("Start creating the initial routes");
            for (int i = 1; i < data.Dimension; i++)
            {
                VehicleRoute tour = new VehicleRoute(i);
                tour.totalDemandOnRoute = data.Demands[i];
                tour.totalDistanceOfRoute = 2 * data.Distances[0, i];
                Tours.Add(tour);
                availNodes.Add(i);
            }
            //Console.WriteLine("Finished creating the initial routes");

            //We will iterate through the savings list
            for (int i = 0; i < Savings.Count; i++)
            {
                //flag variable that indicate if both nodes in savings couple are at the extremites of a tour
                int flag = 0;

                //check if the nodes are in the extremites
                for (int j = 0; j < availNodes.Count; j++)
                    if ((availNodes[j] == Savings[i].FirstNode) || (availNodes[j] == Savings[i].SecondNode))
                        flag++;

                //If the nodes are at the extremities, try to merge
                if (flag == 2)
                {
                    //Console.WriteLine("Trying to merge since nodes are extremeties");
                    //we need to find the two tours that contain the nodes
                    //id of the tours
                    int indexOfFirstRoute = -1;
                    int indexOfSecondRoute = -1;

                    for (int j = 0; j < Tours.Count; j++)
                    {
                        //We found the tour that finishes with the first node
                        if (Tours[j].GetNodeAt(Tours[j].GetNumberOfNodesVisited()) == Savings[i].FirstNode)
                            indexOfFirstRoute = j;
                        //We found the tour that starts with the second node
                        if (Tours[j].GetNodeAt(1) == Savings[i].SecondNode)
                            indexOfSecondRoute = j;
                    }

                    // Merge routes if:
                    // 1. pair from savings finishes one route and starts another
                    // 2. its not the same route
                    // 3. capacity not violated by merging
                    if (indexOfSecondRoute >= 0 && indexOfFirstRoute >= 0 && indexOfSecondRoute != indexOfFirstRoute && Tours[indexOfFirstRoute].totalDemandOnRoute + Tours[indexOfSecondRoute].totalDemandOnRoute <= data.Capacity)
                    {
                        int newlength = Tours[indexOfFirstRoute].GetNumberOfNodesVisited() + Tours[indexOfSecondRoute].GetNumberOfNodesVisited();

                        // Create a new sequence of nodes which corresponds to the merged route. It will include nodes from both routes
                        List<int> tour = new List<int>();
                        double distCalculated = 0;
                        tour.Add(0);
                        int j = 0;
                        for (j = 0; j < newlength; j++)
                        {
                            if (j < Tours[indexOfFirstRoute].GetNumberOfNodesVisited())
                                tour.Add(Tours[indexOfFirstRoute].GetNodeAt(j + 1));
                            else
                            {
                                //subtract the length of tour A
                                int normIndex = j - Tours[indexOfFirstRoute].GetNumberOfNodesVisited();
                                tour.Add(Tours[indexOfSecondRoute].GetNodeAt(normIndex + 1));
                            }
                            distCalculated += data.Distances[tour[j], tour[j + 1]];
                        }
                        distCalculated += data.Distances[tour[j], 0];
                        tour.Add(0);
                        VehicleRoute Tour = new VehicleRoute(tour);
                        Tour.totalDistanceOfRoute = distCalculated;
                        Tour.totalDemandOnRoute = Tours[indexOfFirstRoute].totalDemandOnRoute + Tours[indexOfSecondRoute].totalDemandOnRoute;
                        //add the combined tour and remove the old ones
                        Tours.Add(Tour);

                        // If the any of the tours merged are of length greater than 2, their corresponding extreme (starting or ending) node cannot
                        // be merged anymore. It can be removed from the savings list.
                        if (Tours[indexOfFirstRoute].GetNumberOfNodesVisited() > 1)
                            availNodes.Remove(Tours[indexOfFirstRoute].GetLastNodeVisited());
                        if (Tours[indexOfSecondRoute].GetNumberOfNodesVisited() > 1)
                            availNodes.Remove(Tours[indexOfSecondRoute].GetNodeAt(1));

                        // Remove the merged routes from the route list.
                        Tours.RemoveAt(indexOfFirstRoute);
                        if (indexOfFirstRoute > indexOfSecondRoute)
                            Tours.RemoveAt(indexOfSecondRoute);
                        else
                            Tours.RemoveAt(indexOfSecondRoute - 1);
                    }
                }
            }

            //Tours now contain the solution
            CVRPSolution sol = new CVRPSolution();
            sol.listOfRoutes = new List<VehicleRoute>();

            double heuristicTotalCost = 0;
            for (int i = 0; i < Tours.Count; i++)
            {
                sol.listOfRoutes.Add(Tours[i]);
                heuristicTotalCost += data.Distances[0, sol.listOfRoutes[i].GetNodeAt(1)] + data.Distances[sol.listOfRoutes[i].GetLastNodeVisited(), 0];
                for (int j = 0; j < sol.listOfRoutes[i].GetNumberOfNodesVisited() - 1; j++)
                    heuristicTotalCost += data.Distances[sol.listOfRoutes[i].GetNodeAt(j + 1), sol.listOfRoutes[i].GetNodeAt(j + 2)];
            }
            sol.totalCost = heuristicTotalCost;
            //Console.WriteLine("Heuristic done.\nTotalcost : " + sol.totalCost);
            return sol;
        }

        //this is the randomized savings implementation, credit Imry & Stefan
        public static CVRPSolution RandSavingsMethodHeuristic(CVRPData data)
        {

            //The savings list
            List<SavingsCouple> Savings = new List<SavingsCouple>();
            //Go over all possible couples once and create a coupling, we rely on the fact that the distances are symmetric
            for (int i = 1; i < data.Dimension; i++)
            {
                for (int j = 1; j < data.Dimension; j++)
                {
                    if (j != i)
                    {
                        SavingsCouple cpl = new SavingsCouple();
                        cpl.FirstNode = i;
                        cpl.SecondNode = j;
                        cpl.Saving = data.Distances[i, 0] + data.Distances[0, j] - data.Distances[i, j];
                        Savings.Add(cpl);
                    }
                }

            }

            Savings = Savings.OrderByDescending(x => x.Saving).ToList();

            // Current tour list. Populated in the following loop, and will have tours removed and added as the heuristic progresses.
            List<VehicleRoute> Tours = new List<VehicleRoute>();
            // List of nodes that are either at the start of a tour or at the end. These can be merged. Initially all nodes are in the list.
            List<int> availNodes = new List<int>();
            // Create the inital tours
            //Console.WriteLine("Start creating the initial routes");
            for (int p = 1; p < data.Dimension; p++)
            {
                VehicleRoute tour = new VehicleRoute(p);
                tour.totalDemandOnRoute = data.Demands[p];
                tour.totalDistanceOfRoute = 2 * data.Distances[0, p];
                Tours.Add(tour);
                availNodes.Add(p);
            }
            //Console.WriteLine("Finished creating the initial routes");

            //Randomize savings addition, Credit to Imry & Stefan
            Random rand = new Random();
            while (Savings.Count() > 0)
            {
                // Generate a Poisson Random Variable with mean 1
                double p = 1, L = Math.Exp(-1);
                int k = 0;
                do
                {
                    k++;
                    p = rand.NextDouble();
                }
                while (p > L);
                int i = Math.Min(k - 1, Savings.Count() - 1);
            
                //flag variable that indicate if both nodes in savings couple are at the extremites of a tour
                int flag = 0;

                //check if the nodes are in the extremites
                for (int j = 0; j < availNodes.Count; j++)
                    if ((availNodes[j] == Savings[i].FirstNode) || (availNodes[j] == Savings[i].SecondNode))
                        flag++;

                //If the nodes are at the extremities, try to merge
                if (flag == 2)
                {
                    //Console.WriteLine("Trying to merge since nodes are extremeties");
                    //we need to find the two tours that contain the nodes
                    //id of the tours
                    int indexOfFirstRoute = -1;
                    int indexOfSecondRoute = -1;

                    for (int j = 0; j < Tours.Count; j++)
                    {
                        //We found the tour that finishes with the first node
                        if (Tours[j].GetNodeAt(Tours[j].GetNumberOfNodesVisited()) == Savings[i].FirstNode)
                            indexOfFirstRoute = j;
                        //We found the tour that starts with the second node
                        if (Tours[j].GetNodeAt(1) == Savings[i].SecondNode)
                            indexOfSecondRoute = j;
                    }

                    // Merge routes if:
                    // 1. pair from savings finishes one route and starts another
                    // 2. its not the same route
                    // 3. capacity not violated by merging
                    if (indexOfSecondRoute >= 0 && indexOfFirstRoute >= 0 && indexOfSecondRoute != indexOfFirstRoute && Tours[indexOfFirstRoute].totalDemandOnRoute + Tours[indexOfSecondRoute].totalDemandOnRoute <= data.Capacity)
                    {
                        int newlength = Tours[indexOfFirstRoute].GetNumberOfNodesVisited() + Tours[indexOfSecondRoute].GetNumberOfNodesVisited();

                        // Create a new sequence of nodes which corresponds to the merged route. It will include nodes from both routes
                        List<int> tour = new List<int>();
                        double distCalculated = 0;
                        tour.Add(0);
                        int j = 0;
                        for (j = 0; j < newlength; j++)
                        {
                            if (j < Tours[indexOfFirstRoute].GetNumberOfNodesVisited())
                                tour.Add(Tours[indexOfFirstRoute].GetNodeAt(j + 1));
                            else
                            {
                                //subtract the length of tour A
                                int normIndex = j - Tours[indexOfFirstRoute].GetNumberOfNodesVisited();
                                tour.Add(Tours[indexOfSecondRoute].GetNodeAt(normIndex + 1));
                            }
                            distCalculated += data.Distances[tour[j], tour[j + 1]];
                        }
                        distCalculated += data.Distances[tour[j], 0];
                        tour.Add(0);
                        VehicleRoute Tour = new VehicleRoute(tour);
                        Tour.totalDistanceOfRoute = distCalculated;
                        Tour.totalDemandOnRoute = Tours[indexOfFirstRoute].totalDemandOnRoute + Tours[indexOfSecondRoute].totalDemandOnRoute;
                        //add the combined tour and remove the old ones
                        Tours.Add(Tour);

                        // If the any of the tours merged are of length greater than 2, their corresponding extreme (starting or ending) node cannot
                        // be merged anymore. It can be removed from the savings list.
                        if (Tours[indexOfFirstRoute].GetNumberOfNodesVisited() > 1)
                            availNodes.Remove(Tours[indexOfFirstRoute].GetLastNodeVisited());
                        if (Tours[indexOfSecondRoute].GetNumberOfNodesVisited() > 1)
                            availNodes.Remove(Tours[indexOfSecondRoute].GetNodeAt(1));

                        // Remove the merged routes from the route list.
                        Tours.RemoveAt(indexOfFirstRoute);
                        if (indexOfFirstRoute > indexOfSecondRoute)
                            Tours.RemoveAt(indexOfSecondRoute);
                        else
                            Tours.RemoveAt(indexOfSecondRoute - 1);
                    }
                }
                Savings.RemoveAt(i);
            }

            //Tours now contain the solution
            CVRPSolution sol = new CVRPSolution();
            sol.listOfRoutes = new List<VehicleRoute>();

            double heuristicTotalCost = 0;
            for (int i = 0; i < Tours.Count; i++)
            {
                sol.listOfRoutes.Add(Tours[i]);
                heuristicTotalCost += data.Distances[0, sol.listOfRoutes[i].GetNodeAt(1)] + data.Distances[sol.listOfRoutes[i].GetLastNodeVisited(), 0];
                for (int j = 0; j < sol.listOfRoutes[i].GetNumberOfNodesVisited() - 1; j++)
                    heuristicTotalCost += data.Distances[sol.listOfRoutes[i].GetNodeAt(j + 1), sol.listOfRoutes[i].GetNodeAt(j + 2)];
            }
            sol.totalCost = heuristicTotalCost;
            //check solution feasibility
            //sol.FeasibilityCheck();
            //Console.WriteLine(" Rand Heuristic done.\nTotalcost : " + sol.totalCost);
            return sol;
        }






    }
}
